/* Test of SVN commit */  

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <iostream>

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCapsule dsDrawCapsuleD
#endif

#define DENSITY (5.0)

using namespace std;

static dWorldID      world;
static dSpaceID      space;
static dGeomID       ground;
static dGeomID       vehicle;
static dJointGroupID contactgroup;
static dGeomID ground_box[3];


// things that the user controls

static dReal speed=0,yaw=0;	// user commands

dsFunctions fn;

typedef struct
{
    dBodyID body;
    dGeomID geom;
} MyObject;
MyObject obj;

// Create a capsule
void createCapsule()
{
	dMass m1;
	dReal radius = .25;
	dReal length = 1;

    	obj.body = dBodyCreate (world);
        dMassSetZero (&m1); 
	dMassSetCapsule (&m1,DENSITY,3,radius,length);
	dBodySetMass(obj.body,&m1);
	dBodySetPosition (obj.body,0,0,1.5);



    	dMatrix3 R;	
	dRFromAxisAndAngle (R,1,0,0,M_PI/2);
    	dBodySetRotation (obj.body,R);
	obj.geom = dCreateCapsule (space,radius,length);
	dGeomSetBody(obj.geom,obj.body);

	dGeomSetOffsetPosition(obj.geom,0,0,0);
	dMatrix3 R2;
	dRFromAxisAndAngle (R2,0,0,1,0);
	dGeomSetOffsetRotation(obj.geom,R2);
}

static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
    const int N = 10;
    dContact contact[N];



    int n =  dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
        for (int i = 0; i < n; i++)
        {
            contact[i].surface.mode = dContactBounce|dContactSoftERP|dContactSoftCFM;
            contact[i].surface.bounce = 0.5; 
            contact[i].surface.soft_erp = 1.0;
            contact[i].surface.soft_cfm = 0.0;
            dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
            dJointAttach (c,dGeomGetBody(contact[i].geom.g1),
                          dGeomGetBody(contact[i].geom.g2));
        }
}

void destroyCapsule()
{
    dBodyDestroy(obj.body);
    dGeomDestroy(obj.geom);
}

void drawObject(dGeomID g, dBodyID b, const dReal *pos, const dReal *R, int show_aabb)
{
  int i;
	
  if (!g) return;
  if (!pos) {
	pos = dBodyGetPosition(b);
  }
  if (!R) {
	R = dBodyGetRotation(b);
  }

  int type = dGeomGetClass (g);
  if (type == dCapsuleClass) {
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    dsDrawCapsule (pos,R,length,radius);
  }    
  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}
void restart()
{
    // destroy
    dJointGroupDestroy(contactgroup);
 
    destroyCapsule();

    // create
    contactgroup = dJointGroupCreate(0);

    createCapsule();
}
void command(int cmd)
{
  switch (cmd) {
  case 'w':
  {
	dBodyAddRelForceAtRelPos(obj.body,0,0,2,0,0,-1); /*Applying force at end of vehicle*/

	break;
  }
  case 's': 
  {
	dBodyAddRelForceAtRelPos(obj.body,0,0,-2,0,0,-1);
	break;
  }
  case 'a':
  {
	dBodyAddRelForceAtRelPos(obj.body,cos(45*M_PI/180)*2.0,0,sin(45*M_PI/180)*2.0,0,0,-1);
	break;
  }
  case 'd':
  {
	dBodyAddRelForceAtRelPos(obj.body,-cos(45*M_PI/180)*2.0,0,sin(45*M_PI/180)*2.0,0,0,-1);
	break;
  }
  case 'r':restart();break;
  }

}

static void simLoop (int pause)
{
    const dReal *linVel;
    const dReal *AngVel;
	AngVel = dBodyGetAngularVel(obj.body);
	linVel = dBodyGetLinearVel(obj.body);
if(linVel[0]!=0 || linVel[1]!=0 || linVel[2]!=0)
{
	dReal air_drag[3], dir[3];

	dir[0] = (linVel[0])/sqrt(linVel[0]*linVel[0]+linVel[1]*linVel[1]+linVel[2]*linVel[2]);	
	dir[1] = (linVel[1])/sqrt(linVel[0]*linVel[0]+linVel[1]*linVel[1]+linVel[2]*linVel[2]);
	dir[2] = (linVel[2])/sqrt(linVel[0]*linVel[0]+linVel[1]*linVel[1]+linVel[2]*linVel[2]);
	air_drag[0] = -dir[0]*3.0*(linVel[0]*linVel[0]);
	air_drag[1] = -dir[1]*1.5*(linVel[1]*linVel[1]);
	air_drag[2] = -dir[2]*1.5*(linVel[2]*linVel[2]);
	printf(" X1 = %5.5f X2 = %5.5f y1 = %5.5f y2 = %5.5f z1 = %5.5f z2 = %5.5f \n  xdir = %2.2f ydir = %2.2f zdir = %2.2f\n",linVel[0],air_drag[0],linVel[1],air_drag[1],linVel[2],air_drag[2],dir[0],dir[1],dir[3]);
	dBodyAddForce(obj.body,air_drag[0],air_drag[1],air_drag[2]);
//	dBodyAddForce(obj.body,-.75*linVel[0],-.75*linVel[1],-.75*linVel[2]);
}
if(AngVel[0]!=0 || AngVel[1]!=0 || AngVel[2]!=0) 
{
	dReal air_drag[3], dir[3];
	dir[0] = (AngVel[0])/sqrt(AngVel[0]*AngVel[0]+AngVel[1]*AngVel[1]+AngVel[2]*AngVel[2]);	
	dir[1] = (AngVel[1])/sqrt(AngVel[0]*AngVel[0]+AngVel[1]*AngVel[1]+AngVel[2]*AngVel[2]);
	dir[2] = (AngVel[2])/sqrt(AngVel[0]*AngVel[0]+AngVel[1]*AngVel[1]+AngVel[2]*AngVel[2]);
	air_drag[0] = -dir[0]*1.5*(AngVel[0]*AngVel[0]);
	air_drag[1] = -dir[1]*2.0*(AngVel[1]*AngVel[1]);
	air_drag[2] = -dir[2]*1.5*(AngVel[2]*AngVel[2]);
	dBodyAddTorque(obj.body,air_drag[0],air_drag[1],air_drag[2]);
}
    if (!pause)
    {
        dSpaceCollide(space,0,&nearCallback);
        dWorldStep(world,0.01);
        dJointGroupEmpty(contactgroup);

    }	
    drawObject(obj.geom,obj.body,0,0,0);
    dVector3 ss;
    dGeomBoxGetLengths (ground_box[0],ss);
    dsDrawBox (dGeomGetPosition(ground_box[0]),dGeomGetRotation(ground_box[0]),ss);
    dVector3 ss1;
    dGeomBoxGetLengths (ground_box[1],ss1);
    dsDrawBox (dGeomGetPosition(ground_box[1]),dGeomGetRotation(ground_box[1]),ss1);
    dVector3 ss2;
    dGeomBoxGetLengths (ground_box[2],ss2);
    dsDrawBox (dGeomGetPosition(ground_box[2]),dGeomGetRotation(ground_box[2]),ss2);

}

void start()
{
    static float xyz[3] = {0.0, 0.0,10.0};
    static float hpr[3] = {0.0, -90.0,0.0};
    dsSetViewpoint (xyz,hpr);
}





void  prepDrawStuff()
{
    fn.version = DS_VERSION;
    fn.start   = &start;
    fn.step    = &simLoop;
    fn.command = &command;
    fn.path_to_textures = "../Documents/ode-0.12/drawstuff/textures";
}

int main (int argc, char **argv)
{
    prepDrawStuff();

    dInitODE();

    world = dWorldCreate();
    space = dHashSpaceCreate(0);
    contactgroup = dJointGroupCreate(0);

    dWorldSetGravity(world,0,0,0);

    ground = dCreatePlane(space,0,0,1,0);
 
    createCapsule();    // create a capsule
    
    ground_box[0] = dCreateBox (space,2,2,2);
    dGeomSetPosition (ground_box[0],6,1,2);
    ground_box[1] = dCreateBox (space,2,2,2);
    dGeomSetPosition (ground_box[1],2,2,2);
    ground_box[2] = dCreateBox (space,2,2,2);
    dGeomSetPosition (ground_box[2],-6,2,2);

    dsSimulationLoop (argc,argv,640,480,&fn);

    dJointGroupDestroy(contactgroup);
    dSpaceDestroy(space);
    dWorldDestroy(world);
    dCloseODE();
    return 0;
}
